/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.Recoverable;
import edu.wpi.first.wpilibj.templates.commands.AdjustArmPosition;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.LogitechController;
import edu.wpi.first.wpilibj.templates.OI;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;

/**
 *
 * @author matt
 */
public class ArmSubsystem extends Subsystem implements Recoverable {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    // These values set the upper and lower arm positions
    // Upper is shot position, lower is ingest position
    private final static double m_kUpperArmPositionSensorValue = 0.815;
    private final static double m_kLowerArmPositionSensorValue = 0.300;
    private final static double m_kIntermediateArmPositionSensorValue = 0.500;
    
    // Range of motion in motor coordinates
    private static final double UpperArmLimit = 1.0;
    private static final double LowerArmLimit = 0.0;
    // predefined raised and lowered arm positions
    private static final double position[] = {m_kLowerArmPositionSensorValue, m_kIntermediateArmPositionSensorValue, m_kUpperArmPositionSensorValue};
    private static double commandedPosition = 0.5;
    private final static double m_kPositionTolerance = 0.05;

    public static class ArmPresetLocation {

        public int value;
        // Lifter PresetPositions
        protected static final int m_kLowered = 0;
        protected static final int m_kIntermediate = 1;
        protected static final int m_kRaised = 2;
        public static final ArmPresetLocation kRaised = new ArmPresetLocation(m_kRaised);
        public static final ArmPresetLocation kIntermediate = new ArmPresetLocation(m_kIntermediate);
        public static final ArmPresetLocation kLowered = new ArmPresetLocation(m_kLowered);

        private ArmPresetLocation(int position) {
            this.value = position;
        }
    }
    private CANJaguar m_armDrive = null;
    private int m_armMotorCANid = 0;
    // Try up to 5 times
    private static final int m_kMaxRetries = 5;

    public void initDefaultCommand() {
        // Set the default command for a subsystem here
        setDefaultCommand(new AdjustArmPosition());
    }

    public ArmSubsystem(int motorCANID) {
        System.out.println("Constructing ArmSubsystem");

        m_armMotorCANid = motorCANID;

        int tries = 0;
        do {
            try {
                m_armDrive = new CANJaguar(m_armMotorCANid);
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("ArmSubsystem(): CAN Timeout while creating motor interface for CAN ID " + m_armMotorCANid);
                // set this motor to null since it wouldn't create (that way other parts of the class can tell not to use it)
                m_armDrive = null;
            } catch (Exception all) {
                System.out.println(all.toString());
            }
            tries++;
        } while ((null == m_armDrive) && tries < m_kMaxRetries);
        if ((null == m_armDrive)) {
            System.out.println("ArmSubsystem(): created motor " + m_armMotorCANid);
        }
        System.out.println("Motor Created  - " + m_armMotorCANid);
        initialize();
    }

    public final void initialize() {

        if (null != m_armDrive) {
            double gain_Kp = 5000.0; // Proportional
            double gain_Ki = .1; // Integral
            double gain_Kd = 0.0; // Derivative

            int tries = 0;
            boolean configured = false;
            do {
                try {
                    m_armDrive.changeControlMode(CANJaguar.ControlMode.kPosition);
                    m_armDrive.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
                    m_armDrive.setPID(gain_Kp, gain_Ki, gain_Kd);
                    // Ratios on Swerve Proxy 5*12.75*(14/8)^3 = 341
                    //m_swerveDrive.configPotentiometerTurns(345);  // 1 turn pot
                    m_armDrive.configPotentiometerTurns(1);  // 1 turn pot
                    //m_swerveDrive.configSoftPositionLimits(0, 25);
                    m_armDrive.enableControl(0);
                    configured = true;
                } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                    System.out.println("ArmSubsystem(): CAN Timeout while configuring arm motor CAN ID " + m_armMotorCANid);
                } catch (Exception all) {
                    System.out.println(all.toString());
                }
                tries++;
            } while (!configured && (tries < m_kMaxRetries));
            if (configured) {
                System.out.println("ArmSubsystem(): Configured arm motor " + m_armMotorCANid);
            }
        }
        System.out.println("ArmSubsystem Created with motor - " + m_armMotorCANid);
    }

    public void adjustArm(double adjustment) {        
        commandedPosition = commandedPosition + adjustment;
        System.out.println("ArmSubsystem: Adjust Arm " + adjustment + " to " + commandedPosition);
        setPosition(commandedPosition);
    }

    public void setPosition(ArmPresetLocation thePosition) {
        commandedPosition = position[thePosition.value];
        System.out.println("ArmSubsystem: Lower Arm " + commandedPosition);
        setPosition(commandedPosition);       
    }
    
    public void setPosition(double arbitraryPosition) {
        commandedPosition = arbitraryPosition;
        System.out.println("ArmSubsystem: setPosition(" + commandedPosition + ")");
        SmartDashboard.putDouble("ArmCommandedPosition", commandedPosition);
        if (null != m_armDrive) {
            try {
                System.out.println("Moving to " + commandedPosition);
                m_armDrive.setX(commandedPosition);
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("ArmSubsystem.setPosition(): CAN Timeout while setting arm motor position" + m_armMotorCANid);
            }
        }
    }

    public double getPosition() {
        double position = 0;
        if (null != m_armDrive) {
            try {
                position = m_armDrive.getPosition();
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("ArmSubsystem.getPosition(): CAN Timeout getting postion on motor " + m_armMotorCANid);
            }
        }
        return position;
    }

    public boolean isLowered() {
        double delta = getPosition() - position[ArmPresetLocation.kLowered.value];
        return (Math.abs(delta) < m_kPositionTolerance);
    }

    public double getCommandedPosition() {
        return commandedPosition;
    }

    /*
     * Power drop recovery support
     */
    public void recover(boolean override) {
        boolean needsRecovery = false;

        // figure out if the Jaguar has rebooted
        try {
            if (null != m_armDrive) {
                needsRecovery = m_armDrive.getPowerCycled();
            }
        } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
            System.out.println("ArmSubsystem(): CAN Timeout while configuring arm CAN ID " + m_armMotorCANid);
        }

        // and if it did, reinitialize
        if (needsRecovery || override) {
            initialize();
        }
    }
}
